/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2008, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
*********************************************************************/

#include <base_local_planner/trajectory_planner.h>
#include <costmap_2d/footprint.h>
#include <string>
#include <sstream>
#include <math.h>
#include <angles/angles.h>


#include <boost/algorithm/string.hpp>

#include <ros/console.h>

//for computing path distance
#include <queue>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/utils.h>

using namespace std;
using namespace costmap_2d;

namespace base_local_planner
{

void TrajectoryPlanner::reconfigure(BaseLocalPlannerConfig &cfg)
{
    BaseLocalPlannerConfig config(cfg);
    boost::mutex::scoped_lock l(configuration_mutex_);
    acc_lim_x_ = config.acc_lim_x;
    acc_lim_y_ = config.acc_lim_y;
    acc_lim_theta_ = config.acc_lim_theta;
    max_vel_x_ = config.max_vel_x;
    min_vel_x_ = config.min_vel_x;
    max_vel_th_ = config.max_vel_theta;
    min_vel_th_ = config.min_vel_theta;
    min_in_place_vel_th_ = config.min_in_place_vel_theta;
    sim_time_ = config.sim_time;
    sim_granularity_ = config.sim_granularity;
    angular_sim_granularity_ = config.angular_sim_granularity;
    path_distance_bias_ = config.path_distance_bias;
    goal_distance_bias_ = config.goal_distance_bias;
    occdist_scale_ = config.occdist_scale;
    if (meter_scoring_)
    {
        // if we use meter scoring, then we want to multiply the biases by the resolution of the costmap
        // 如果我们使用计分法，那么我们需要将偏差乘以成本图的分辨率
        double resolution = costmap_.getResolution();
        goal_distance_bias_ *= resolution;
        path_distance_bias_ *= resolution;
    }
    oscillation_reset_dist_ = config.oscillation_reset_dist;
    escape_reset_dist_ = config.escape_reset_dist;
    escape_reset_theta_ = config.escape_reset_theta;
    vx_samples_ = config.vx_samples;
    vtheta_samples_ = config.vtheta_samples;
    if (vx_samples_ <= 0)
    {
        config.vx_samples = 1;
        vx_samples_ = config.vx_samples;
        ROS_WARN("You've specified that you don't want any samples in the x dimension. "
                 "We'll at least assume that you want to sample one value... so we're going to set vx_samples to 1 instead");
    }
    if (vtheta_samples_ <= 0)
    {
        config.vtheta_samples = 1;
        vtheta_samples_ = config.vtheta_samples;
        ROS_WARN("You've specified that you don't want any samples in the theta dimension. "
                 "We'll at least assume that you want to sample one value... so we're going to set vtheta_samples to 1 instead");
    }
    heading_lookahead_ = config.heading_lookahead;
    holonomic_robot_ = config.holonomic_robot;
    backup_vel_ = config.escape_vel;
    dwa_ = config.dwa;
    heading_scoring_ = config.heading_scoring;
    heading_scoring_timestep_ = config.heading_scoring_timestep;
    simple_attractor_ = config.simple_attractor;
    //y-vels
    string y_string = config.y_vels;
    vector<string> y_strs;
    boost::split(y_strs, y_string, boost::is_any_of(", "), boost::token_compress_on);
    vector<double> y_vels;
    for (auto & y_str : y_strs)
    {
        istringstream iss(y_str);
        double temp;
        iss >> temp;
        y_vels.push_back(temp);
        //ROS_INFO("Adding y_vel: %e", temp);
    }
    y_vels_ = y_vels;
}

/**
 * @brief 构造路径规划器
 * @param world_model  路径规划器用来碰撞检测的世界模型
 * @param costmap 局部规划器用到的代价地图的引用
 * @param footprint_spec  代表机器人的多边形(必须是凸的)
 * @param inscribed_radius  机器人内切圆的半径
 * @param circumscribed_radius  机器人外接圆的半径
 * @param acc_lim_x 机器人在x方向上的加速度限制
 * @param acc_lim_y 机器人在y方向上的加速度限制
 * @param acc_lim_theta  角速度限制
 * @param sim_time The number of seconds to "roll-out" each trajectory "roll-out"每个局部路径的秒数
 * @param sim_granularity 仿真点之间的距离应该足够的小而不会撞到障碍物
 * @param vx_samples  在x方向采样的局部路径数量？
 * @param vtheta_samples  在旋转方向采样的局部路径数量
 * @param path_distance_bias 调节机器人应该离全局路径多近的尺度因子
 * @param goal_distance_bias 调节机器人应该多'积极'地导航去目标点的尺度因子
 * @param occdist_scale  调节机器人离障碍物距离尺度因子
 * @param heading_lookahead How far the robot should look ahead of itself when differentiating between different rotational velocities？
 * @param oscillation_reset_dist The distance the robot must travel before it can explore rotational velocities that were unsuccessful in the past
 * @param escape_reset_dist 退出逃跑模式之前机器人要走过的距离长度
 * @param escape_reset_theta 退出逃跑模式之前机器人要旋转的角度阈值
 * @param holonomic_robot 如果为true, 机器人也会被y方向的线速度控制
 * @param max_vel_x 最大线速度
 * @param min_vel_x 最小线速度
 * @param max_vel_th 最大角速度
 * @param min_vel_th 最小角速度
 * @param min_in_place_vel_th 最小定点旋转角速度 The absolute value of the minimum in-place rotational velocity the controller will explore
 * @param backup_vel 后退速度？ The velocity to use while backing up
 * @param dwa 如果为true, 将用动态窗口法DWA
 * @param heading_scoring  true表示轨迹打分项会包括机器人朝向
 * @param heading_scoring_timestep 从多远开始为局部轨迹的朝向打分
 * @param meter_scoring 调节参数和代价地图的分辨率适配
 * @param simple_attractor true允许 to allow simple attraction to a goal point instead of intelligent cost propagation
 * @param y_vels y方向线速度的向量
 * @param angular_sim_granularity 仿真点之间的距离应该足够的小而不会撞到障碍物？
 */
TrajectoryPlanner::TrajectoryPlanner(WorldModel &world_model, const Costmap2D &costmap,
                                     std::vector<geometry_msgs::Point> footprint_spec,
                                     double acc_lim_x, double acc_lim_y, double acc_lim_theta,
                                     double sim_time, double sim_granularity, int vx_samples, int vtheta_samples,
                                     double path_distance_bias, double goal_distance_bias, double occdist_scale,
                                     double heading_lookahead, double oscillation_reset_dist, double escape_reset_dist,
                                     double escape_reset_theta, bool holonomic_robot, double max_vel_x, double min_vel_x,
                                     double max_vel_th, double min_vel_th, double min_in_place_vel_th, double backup_vel,
                                     bool dwa, bool heading_scoring, double heading_scoring_timestep,
                                     bool meter_scoring, bool simple_attractor, vector<double> y_vels,
                                     double stop_time_buffer, double sim_period, double angular_sim_granularity)
        : path_map_(costmap.getSizeInCellsX(), costmap.getSizeInCellsY()),
          goal_map_(costmap.getSizeInCellsX(), costmap.getSizeInCellsY()),
          costmap_(costmap),
          world_model_(world_model), footprint_spec_(footprint_spec),
          sim_time_(sim_time), sim_granularity_(sim_granularity), angular_sim_granularity_(angular_sim_granularity),
          vx_samples_(vx_samples), vtheta_samples_(vtheta_samples),
          path_distance_bias_(path_distance_bias), goal_distance_bias_(goal_distance_bias),
          occdist_scale_(occdist_scale),
          acc_lim_x_(acc_lim_x), acc_lim_y_(acc_lim_y), acc_lim_theta_(acc_lim_theta),
          prev_x_(0), prev_y_(0), escape_x_(0), escape_y_(0), escape_theta_(0),
          heading_lookahead_(heading_lookahead),
          oscillation_reset_dist_(oscillation_reset_dist), escape_reset_dist_(escape_reset_dist),
          escape_reset_theta_(escape_reset_theta), holonomic_robot_(holonomic_robot),
          max_vel_x_(max_vel_x), min_vel_x_(min_vel_x),
          max_vel_th_(max_vel_th), min_vel_th_(min_vel_th), min_in_place_vel_th_(min_in_place_vel_th),
          backup_vel_(backup_vel),
          dwa_(dwa), heading_scoring_(heading_scoring), heading_scoring_timestep_(heading_scoring_timestep),
          simple_attractor_(simple_attractor), y_vels_(y_vels), stop_time_buffer_(stop_time_buffer),
          sim_period_(sim_period)
{
    // the robot is not stuck to begin with 机器人一开始没有被卡住
    stuck_left = false;
    stuck_right = false;
    stuck_left_strafe = false;
    stuck_right_strafe = false;
    rotating_left = false;
    rotating_right = false;
    strafe_left = false;
    strafe_right = false;
    escaping_ = false;
    final_goal_position_valid_ = false;
    costmap_2d::calculateMinAndMaxDistances(footprint_spec_, inscribed_radius_, circumscribed_radius_);
}

TrajectoryPlanner::~TrajectoryPlanner() {}

bool TrajectoryPlanner::getCellCosts(int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost,
                                     float &total_cost)
{
    MapCell cell = path_map_(cx, cy);
    MapCell goal_cell = goal_map_(cx, cy);
    if (cell.within_robot) return false;
    occ_cost = costmap_.getCost(cx, cy);
    if (cell.target_dist == path_map_.obstacleCosts() ||
        cell.target_dist == path_map_.unreachableCellCosts() ||
        occ_cost >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE) return false;
    path_cost = cell.target_dist;
    goal_cost = goal_cell.target_dist;
    total_cost = path_distance_bias_ * path_cost + goal_distance_bias_ * goal_cost + occdist_scale_ * occ_cost;
    return true;
}

void TrajectoryPlanner::generateTrajectory(
        double x, double y, double theta, double vx, double vy, double vtheta,
        double vx_samp, double vy_samp, double vtheta_samp, double acc_x, double acc_y, double acc_theta,
        double impossible_cost, Trajectory &traj)
{
    // make sure the configuration doesn't change mid run
    boost::mutex::scoped_lock l(configuration_mutex_);
    // 记录初始时刻的位姿、速度、角速度
    double x_i = x;
    double y_i = y;
    double theta_i = theta;
    double vx_i, vy_i, vtheta_i;
    vx_i = vx;
    vy_i = vy;
    vtheta_i = vtheta;
    // compute the magnitude of the velocities 非全向机器人即线速度
    double vmag = hypot(vx_samp, vy_samp);
    // compute the number of steps we must take along this trajectory to be "safe"
    int num_steps;
    // sim_granularity_：仿真点之间的距离间隔
    // 步数 = max(速度模 × 总仿真时间 / 距离间隔，角速度 / 角速度间隔)，四舍五入
    if (!heading_scoring_) num_steps = int(
                max((vmag * sim_time_) / sim_granularity_, fabs(vtheta_samp) / angular_sim_granularity_) + 0.5);
        // 步数 = 总仿真时间/距离间隔，四舍五入
    else num_steps = int(sim_time_ / sim_granularity_ + 0.5);
    // we at least want to take one step... even if we won't move, we want to score our current position
    // 至少一步
    if (num_steps == 0) num_steps = 1;
    // 每一步的时间
    double dt = sim_time_ / num_steps;
    double time = 0.0;
    // create a potential trajectory  声明轨迹
    traj.resetPoints();
    traj.xv_ = vx_samp;         // 线速度
    traj.yv_ = vy_samp;
    traj.thetav_ = vtheta_samp; // 角速度
    traj.cost_ = -1.0;          // 默认代价-1.0
    // initialize the costs for the trajectory 初始化轨迹的代价
    double path_dist = 0.0;     // 路径距离
    double goal_dist = 0.0;     // 目标距离
    double occ_cost = 0.0;      // 障碍物代价
    double heading_diff = 0.0;  // 航向角
    for (int i = 0; i < num_steps; ++i)
    {
        // get map coordinates of a point 存储点的地图上的坐标
        unsigned int cell_x, cell_y;
        // we don't want a path that goes off the know map
        // 不希望路径跑出已知地图, 从当前位置开始，依次循环全局路径上的点
        // 把当前位置(x_i,y_i)转换到地图上，如果无法转换，说明该路径点不在地图上，将其代价设置为-1.0，并return
        if (!costmap_.worldToMap(x_i, y_i, cell_x, cell_y))
        {
            traj.cost_ = -1.0;
            return;
        }
        // check the point on the trajectory for legality
        // 检查当前路径上的点的合法性，每一个路径点→机器人的足迹，检测足迹的多边形里是否碰到障碍物，若遇障则返回负值
        double footprint_cost = footprintCost(x_i, y_i, theta_i);
        // if the footprint hits an obstacle this trajectory is invalid
        if (footprint_cost < 0)
        {
            traj.cost_ = -1.0;
            return;
            //TODO: Really look at getMaxSpeedToStopInTime... dues to discretization errors and high acceleration limits,
            // it can actually cause the robot to hit obstacles.
            // There may be something to be done to fix, but I'll have to come back to it when I have time.
            // Right now, pulling it out as it'll just make the robot a bit more conservative, but safe.
            /*
            double max_vel_x, max_vel_y, max_vel_th;
            //we want to compute the max allowable speeds to be able to stop
            //to be safe... we'll make sure we can stop some time before we actually hit
            getMaxSpeedToStopInTime(time - stop_time_buffer_ - dt, max_vel_x, max_vel_y, max_vel_th);

            //check if we can stop in time
            if(fabs(vx_samp) < max_vel_x && fabs(vy_samp) < max_vel_y && fabs(vtheta_samp) < max_vel_th){
              ROS_ERROR("v: (%.2f, %.2f, %.2f), m: (%.2f, %.2f, %.2f) t:%.2f, st: %.2f, dt: %.2f", vx_samp, vy_samp, vtheta_samp, max_vel_x, max_vel_y, max_vel_th, time, stop_time_buffer_, dt);
              //if we can stop... we'll just break out of the loop here.. no point in checking future points
              break;
            }
            else{
              traj.cost_ = -1.0;
              return;
            }
            */
        }
        // 更新occ_cost：max(max(occ_cost，路径足迹代价)，当前位置的代价)
        // 也就是把所有路径点的最大障碍物代价设置为路径的occ_cost
        occ_cost = std::max(std::max(occ_cost, footprint_cost), double(costmap_.getCost(cell_x, cell_y)));
        // do we want to follow blindly
        /*********************这里负责更新路径和目标距离**********************/
        // 如果只是想blindly follow的话，如下（求与目标间的三角距离），否则见后
        // 如果simple_attractor_开启，采用比较简单的追踪策略，只考虑与目标点之间的直线距离^2
        if (simple_attractor_)
        {
            // 目标距离
            goal_dist = (x_i - global_plan_[global_plan_.size() - 1].pose.position.x) *
                        (x_i - global_plan_[global_plan_.size() - 1].pose.position.x) +
                        (y_i - global_plan_[global_plan_.size() - 1].pose.position.y) *
                        (y_i - global_plan_[global_plan_.size() - 1].pose.position.y);
        }
        // 如果simple_attractor_未开启，则借助goal_map_和path_map_获取该点与目标点及全局规划路径之间的距离，
        // 若超过范围，说明没有明确的目标路径，则该路径无效，直接返回-2.0
        else
        {
            // 不只是blindly follow，“更新路径距离和目标距离”
            bool update_path_and_goal_distances = true;
            // with heading scoring, we take into account heading diff,
            // and also only score path and goal distance for one point of the trajectory
            // 如果开启heading_scoring_即为朝向打分，只有当从开始生成该条路径计时，到达特定时刻时，才会进行唯一一次打分
            if (heading_scoring_)
            {
                /**********这里求出本次时间是否是要给航向打分的时间**********/
                // heading_scoring_timestep_是给朝向打分时在时间上要看多远
                // 也就是在路径上走过一个特定时刻后，才为朝向打分一次
                // 如果heading_scoring_timestep_ ≤ 时间 < heading_scoring_timestep_ + 一次的时间间隔
                if (time >= heading_scoring_timestep_ && time < heading_scoring_timestep_ + dt)
                {
                    // 求出航向角
                    heading_diff = headingDiff(cell_x, cell_y, x_i, y_i, theta_i);
                }
                    // 不更新路径和目标距离
                else update_path_and_goal_distances = false;
            }
            if (update_path_and_goal_distances)
            {
                // update path and goal distances 更新路径距离与目标距离（只考虑终点cell）
                path_dist = path_map_(cell_x, cell_y).target_dist;
                goal_dist = goal_map_(cell_x, cell_y).target_dist;
                // if a point on this trajectory has no clear path to goal it is invalid
                // 如果目标距离或路径距离≥impossible_cost（地图尺寸），代价设置为-2.0
                if (impossible_cost <= goal_dist || impossible_cost <= path_dist)
                {
//            ROS_DEBUG("No path to goal with goal distance = %f, path_distance = %f and max cost = %f",
//                goal_dist, path_dist, impossible_cost);
                    traj.cost_ = -2.0;
                    return;
                }
            }
        }
        // 需要注意的是，这里对速度的计算与我们发布给机器人的速度无关，这里的速度只是为了推算下一个点，获得路径，而我们真正发布给机器人的速度是采样速度。
        // 真实世界里机器人由当前速度 –> 采样速度的过程对应我们地图上本次仿真的轨迹。
        // the point is legal... add it to the trajectory 这个点有效，加入轨迹
        traj.addPoint(x_i, y_i, theta_i);
        // calculate velocities 计算速度、角速度
        // computeNewVelocity函数作用：不管速度大于还是小于vx_samp，都让其以加速度acc_x接近vx_samp后稳定在vx_samp
        vx_i = computeNewVelocity(vx_samp, vx_i, acc_x, dt);
        vy_i = computeNewVelocity(vy_samp, vy_i, acc_y, dt);
        vtheta_i = computeNewVelocity(vtheta_samp, vtheta_i, acc_theta, dt);
        // calculate positions 通过计算出的速度计算下一个位置、姿态
        x_i = computeNewXPosition(x_i, vx_i, vy_i, theta_i, dt);
        y_i = computeNewYPosition(y_i, vx_i, vy_i, theta_i, dt);
        theta_i = computeNewThetaPosition(theta_i, vtheta_i, dt);
        // increment time
        time += dt;
    } // end for i < numsteps
    // ROS_INFO("OccCost: %f, vx: %.2f, vy: %.2f, vtheta: %.2f", occ_cost, vx_samp, vy_samp, vtheta_samp);
    double cost = -1.0;
    if (!heading_scoring_)
    {
        cost = path_distance_bias_ * path_dist + goal_dist * goal_distance_bias_ + occdist_scale_ * occ_cost;
    }
    else
    {
        cost = occdist_scale_ * occ_cost + path_distance_bias_ * path_dist + 0.3 * heading_diff +
               goal_dist * goal_distance_bias_;
    }
    traj.cost_ = cost;
}

double TrajectoryPlanner::headingDiff(int cell_x, int cell_y, double x, double y, double heading)
{
    unsigned int goal_cell_x, goal_cell_y;
    // find a clear line of sight from the robot's cell to a farthest point on the path
    for (int i = global_plan_.size() - 1; i >= 0; --i)
    {
        if (costmap_.worldToMap(global_plan_[i].pose.position.x, global_plan_[i].pose.position.y,
                goal_cell_x, goal_cell_y))
        {
            if (lineCost(cell_x, goal_cell_x, cell_y, goal_cell_y) >= 0)
            {
                double gx, gy;
                costmap_.mapToWorld(goal_cell_x, goal_cell_y, gx, gy);
                return fabs(angles::shortest_angular_distance(heading, atan2(gy - y, gx - x)));
            }
        }
    }
    return DBL_MAX;
}

// calculate the cost of a ray-traced line 计算光线追踪线的代价值
double TrajectoryPlanner::lineCost(int x0, int x1, int y0, int y1)
{
    // Bresenham Ray-Tracing
    int deltax = abs(x1 - x0);        // The difference between the x's
    int deltay = abs(y1 - y0);        // The difference between the y's
    int x = x0;                       // Start x off at the first pixel
    int y = y0;                       // Start y off at the first pixel
    int xinc1, xinc2, yinc1, yinc2;
    int den, num, numadd, numpixels;
    double line_cost = 0.0;
    double point_cost = -1.0;
    // The x-values are increasing
    if (x1 >= x0)
    {
        xinc1 = 1;
        xinc2 = 1;
    }
        // The x-values are decreasing
    else
    {
        xinc1 = -1;
        xinc2 = -1;
    }
    // The y-values are increasing
    if (y1 >= y0)
    {
        yinc1 = 1;
        yinc2 = 1;
    }
        // The y-values are decreasing
    else
    {
        yinc1 = -1;
        yinc2 = -1;
    }

    if (deltax >= deltay)         // There is at least one x-value for every y-value
    {
        xinc1 = 0;                  // Don't change the x when numerator >= denominator
        yinc2 = 0;                  // Don't change the y for every iteration
        den = deltax;
        num = deltax / 2;
        numadd = deltay;
        numpixels = deltax;         // There are more x-values than y-values
    }
        // There is at least one y-value for every x-value
    else
    {
        xinc2 = 0;                  // Don't change the x for every iteration
        yinc1 = 0;                  // Don't change the y when numerator >= denominator
        den = deltay;
        num = deltay / 2;
        numadd = deltax;
        numpixels = deltay;         // There are more y-values than x-values
    }

    for (int curpixel = 0; curpixel <= numpixels; curpixel++)
    {
        point_cost = pointCost(x, y); //Score the current point
        if (point_cost < 0) return -1;
        if (line_cost < point_cost) line_cost = point_cost;
        num += numadd;              // Increase the numerator by the top of the fraction
        // Check if numerator >= denominator
        if (num >= den)
        {
            num -= den;               // Calculate the new numerator value
            x += xinc1;               // Change the x as appropriate
            y += yinc1;               // Change the y as appropriate
        }
        x += xinc2;                 // Change the x as appropriate
        y += yinc2;                 // Change the y as appropriate
    }
    return line_cost;
}

double TrajectoryPlanner::pointCost(int x, int y)
{
    unsigned char cost = costmap_.getCost(x, y);
    //if the cell is in an obstacle the path is invalid
    if (cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE || cost == NO_INFORMATION) return -1;
    return cost;
}

void TrajectoryPlanner::updatePlan(const vector<geometry_msgs::PoseStamped> &new_plan, bool compute_dists)
{
    global_plan_.resize(new_plan.size());
    for (unsigned int i = 0; i < new_plan.size(); ++i) global_plan_[i] = new_plan[i];
    if (!global_plan_.empty())
    {
        geometry_msgs::PoseStamped &final_goal_pose = global_plan_[global_plan_.size() - 1];
        final_goal_x_ = final_goal_pose.pose.position.x;
        final_goal_y_ = final_goal_pose.pose.position.y;
        final_goal_position_valid_ = true;
    }
    else final_goal_position_valid_ = false;
    // compute_dists默认为false，即局部规划器在更新全局plan时，不重新计算path_map_和goal_map_
    if (compute_dists)
    {
        // reset the map for new operations
        path_map_.resetPathDist();
        goal_map_.resetPathDist();
        // make sure that we update our path based on the global plan and compute costs
        // 确保我们根据全局路径和计算代价值更新路径
        path_map_.setTargetCells(costmap_, global_plan_);
        goal_map_.setLocalGoal(costmap_, global_plan_);
        ROS_DEBUG("Path/Goal distance computed");
    }
}

bool TrajectoryPlanner::checkTrajectory(double x, double y, double theta, double vx, double vy,
                                        double vtheta, double vx_samp, double vy_samp, double vtheta_samp)
{
    double cost = scoreTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp);
    // if the trajectory is a legal one... the check passes
    if (cost >= 0) return true;
    ROS_WARN("Invalid Trajectory %f, %f, %f, cost: %f", vx_samp, vy_samp, vtheta_samp, cost);
    // otherwise the check fails
    return false;
}

double TrajectoryPlanner::scoreTrajectory(double x, double y, double theta, double vx, double vy,
                                          double vtheta, double vx_samp, double vy_samp, double vtheta_samp)
{
    Trajectory t;
    double impossible_cost = path_map_.obstacleCosts();
    // 生成单条路径并返回代价。它们是在足够接近目标时，局部规划器产生降速和自转时生成的对应速度的路径。
    generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
                       acc_lim_x_, acc_lim_y_, acc_lim_theta_, impossible_cost, t);
    // return the cost.
    return double(t.cost_);
}

// 创建我们要打分的局部轨迹规划
Trajectory TrajectoryPlanner::createTrajectories(double x, double y, double theta,
                                                 double vx, double vy, double vtheta,
                                                 double acc_x, double acc_y, double acc_theta)
{
    // compute feasible velocity limits in robot space 计算机器人空间的可行速度限制 声明最大/小线速度，最大/小角速度
    double max_vel_x = max_vel_x_, max_vel_theta;
    double min_vel_x, min_vel_theta;
    // 首先，计算可行的线速度和角速度范围，这里先对最大线速度进行一个限制，即保证速度既不超过预设的最大速度限制，也不超过“起点与目标直线距离/总仿真时间”。
    // 如果最终的目标是有效的（全局规划不为空）
    if (final_goal_position_valid_)
    {
        // 计算当前位置和目标位置之间的距离：final_goal_dist
        double final_goal_dist = hypot(final_goal_x_ - x, final_goal_y_ - y);
        // 最大速度：在预设的最大速度和
        max_vel_x = min(max_vel_x, final_goal_dist / sim_time_);
    }
    // should we use the dynamic window approach? 是否用动态窗口方法
    // ① 使用dwa法，则用的是轨迹前向模拟的周期sim_period_（1 / 控制频率）；
    // ② 不使用dwa法，则用的是整段仿真时间sim_time_
    if (dwa_)
    {
        // 使用dwa窗口法，sim_period_是dwa计算最大、最小速度用的时间
        // 计算速度、角速度范围，引入加速度限制条件（用sim_period_）
        max_vel_x = max(min(max_vel_x, vx + acc_x * sim_period_), min_vel_x_);
        min_vel_x = max(min_vel_x_, vx - acc_x * sim_period_);
        max_vel_theta = min(max_vel_th_, vtheta + acc_theta * sim_period_);
        min_vel_theta = max(min_vel_th_, vtheta - acc_theta * sim_period_);
    }
        // 不使用dwa
    else
    {
        // 计算速度、角速度范围，引入加速度限制条件（用sim_time_）
        max_vel_x = max(min(max_vel_x, vx + acc_x * sim_time_), min_vel_x_);
        min_vel_x = max(min_vel_x_, vx - acc_x * sim_time_);
        max_vel_theta = min(max_vel_th_, vtheta + acc_theta * sim_time_);
        min_vel_theta = max(min_vel_th_, vtheta - acc_theta * sim_time_);
    }
    // we want to sample the velocity space regularly 对速度空间进行常规的采样
    // 接下来根据预设的线速度与角速度的采样数，和上面计算得到的范围，分别计算出采样间隔，并把范围内最小的线速度和角速度作为初始采样速度。
    // 不考虑全向机器人的情况，即不能y向移动，故暂不考虑vy上采样。
    double dvx = (max_vel_x - min_vel_x) / (vx_samples_ - 1);                 // 计算速度采样间隔
    double dvtheta = (max_vel_theta - min_vel_theta) / (vtheta_samples_ - 1); // 计算角速度采样间隔
    double vx_samp = min_vel_x;          // x方向速度采样点
    double vtheta_samp = min_vel_theta;  // 角速度采样点
    double vy_samp = 0.0;
    // keep track of the best trajectory seen so far
    // 为了迭代比较不同采样速度生成的不同路径的代价，这里声明best_traj和comp_traj并都将其代价初始化为-1
    Trajectory *best_traj = &traj_one;
    Trajectory *comp_traj = &traj_two; // 下面生成的轨迹放到这里，和best_traj作比较，如果生成的轨迹代价更小，就选择它
    Trajectory *swap = NULL;           // 用于交换的指针
    best_traj->cost_ = -1.0;           // 先初始化一个最优路径的代价 = -1
    comp_traj->cost_ = -1.0;           // 先初始化一个当前“对比”路径的代价=-1，等会再用generateTrajectory函数生成路径和新的代价存放在里面
    // any cell with a cost greater than the size of the map is impossible 单元格不应该超过地图的大小
    double impossible_cost = path_map_.obstacleCosts();
    // if we're performing an escape we won't allow moving forward
    // 如果机器人没有处于脱困状态时，开始遍历所有线速度和角速度，调用类内generateTrajectory函数用它们生成轨迹。
    // 二重迭代时，外层遍历线速度（正值），内层遍历角速度。在遍历时，单独拎出角速度=0，即直线前进的情况，避免由于采样间隔的设置而跃过了这种特殊情况。
    // 边迭代生成边比较，获得代价最小的路径，存放在best_traj。 执行完成后，也跳出了“未处于脱困状态”这个判断结构。
    if (!escaping_)
    {
        // loop through all x velocities 遍历所有的 x 速度
        // 外侧循环所有x速度 循环所有速度和角速度、打分
        for (int i = 0; i < vx_samples_; ++i)
        {
            // x速度循环内部遍历角速度
            // ①角速度 = 0
            vtheta_samp = 0;
            // first sample the straight trajectory 首先采样直的局部路径【generateTrajectory函数如何实现-重点！】
            // 传入当前位姿、速度、加速度限制，采样起始x向速度、y向速度、角速度，代价赋给comp_traj
            generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
                               acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
            // if the new trajectory is better... let's take it 采用新的局部路径如果新的更好
            // 对比生成路径和当前最优路径的分数，如果生成的路径分数更小，就把当前路径和最优路径交换
            // 这里会将第一次生成路径的代价赋给best_traj
            if (comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0))
            {
                swap = best_traj;
                best_traj = comp_traj;
                comp_traj = swap;
            }
            // ②角速度 = 下界
            vtheta_samp = min_vel_theta;
            // next sample all theta trajectories 接下来迭代循环生成所有的角速度局部路径、打分
            for (int j = 0; j < vtheta_samples_ - 1; ++j)
            {
                generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
                                   acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
                // if the new trajectory is better... let's take it 采用新的局部路径如果新的更好
                // 同样地，如果新路径代价更小，和best_traj作交换
                if (comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0))
                {
                    swap = best_traj;
                    best_traj = comp_traj;
                    comp_traj = swap;
                }
                // 遍历角速度
                vtheta_samp += dvtheta;
            }
            // 遍历x速度
            vx_samp += dvx;
        }
        // only explore y velocities with holonomic robots 只对全向机器人考虑y方向的速度
        if (holonomic_robot_)
        {
            // explore trajectories that move forward but also strafe slightly
            // 探索前进的轨迹，但也有轻微的偏离
            vx_samp = 0.1;
            vy_samp = 0.1;
            vtheta_samp = 0.0;
            generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
                               acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
            // if the new trajectory is better... let's take it 采用新的局部路径如果新的更好
            if (comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0))
            {
                swap = best_traj;
                best_traj = comp_traj;
                comp_traj = swap;
            }
            vx_samp = 0.1;
            vy_samp = -0.1;
            vtheta_samp = 0.0;
            generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
                               acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
            // if the new trajectory is better... let's take it 采用新的局部路径如果新的更好
            if (comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0))
            {
                swap = best_traj;
                best_traj = comp_traj;
                comp_traj = swap;
            }
        }
    } // end if not escaping
    /**
     * 接下来，继续考虑线速度=0（原地旋转）的情况，来考虑一下这种情况什么时候会发生：
            - 当TrajectoryPlannerROS中，位置已经到达目标（误差范围内），姿态已达，则直接发送0速；姿态未达，则调用降速函数和原地旋转函数，
              并调用checkTrajectory函数检查合法性，直到旋转至目标姿态。而checkTrajectory函数调用的是scoreTrajectory和generateTrajectory，
              不会调用createTrajectory函数，所以，快要到达目标附近时的原地旋转，根本不会进入到这个函数的这部分来处理。
            - 并且，由于局部规划器的路径打分机制（后述）是：“与目标点的距离”和“与全局路径的偏离”这两项打分都只考虑路径终点的cell，
              而不是考虑路径上所有cell的综合效果，机器人运动到一个cell上，哪怕有任何一条能向目标再前进的无障碍路径，
              它的最终得分一定是要比原地旋转的路径得分来得高的。
        所以，这里的原地自转，是行进过程中的、未达目标附近时的原地自转，并且，是机器人行进过程中遇到障碍、前方无路可走只好原地自转，
            或是连原地自转都不能满足，要由脱困状态后退一段距离，再原地自转调整方向，准备接下来的行动。
            一种可能情况是机器人行进前方遇到了突然出现而不在地图上的障碍。
        在处理这种情况时，由于机器人原地旋转时的角速度限制范围要比运动时的角速度限制范围更严格，底盘无法处理过小的原地转速，故要注意处理这层限制。
            同样调用generateTrajectory函数，生成路径。
     */
    // next we want to generate trajectories for rotating in place 产生原地旋转的局部路径
    vtheta_samp = min_vel_theta;
    vx_samp = 0.0;
    vy_samp = 0.0;
    // let's try to rotate toward open space
    double heading_dist = DBL_MAX;
    // 循环所有角速度
    for (int i = 0; i < vtheta_samples_; ++i)
    {
        // enforce a minimum rotational velocity because the base can't handle small in-place rotations
        // 强制最小原地旋转速度
        double vtheta_samp_limited = vtheta_samp > 0 ? max(vtheta_samp, min_in_place_vel_th_)
                                                     : min(vtheta_samp, -1.0 * min_in_place_vel_th_);
        // 产生遍历角速度的路径
        generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp_limited,
                           acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
        // if the new trajectory is better... let's take it...
        // note if we can legally rotate in place we prefer to do that rather than move with y velocity
        // 如果新的局部轨迹更好，采用新的; 注意如果机器人可以定点旋转，则优先定点转而不是在y方向运动
        // TrajectoryPlanner的路径打分策略是以下三者的综合： ①与目标点距离、②与全局规划的距离、③障碍物，
        //  在上述原地自转情况下，可能是突发障碍物，此时①没有意义，而这种筛选策略能够将②和③纳入考虑，也比较合理。
        if (comp_traj->cost_ >= 0 &&
           (comp_traj->cost_ <= best_traj->cost_ || best_traj->cost_ < 0 || best_traj->yv_ != 0.0) &&
           (vtheta_samp > dvtheta || vtheta_samp < -1 * dvtheta))
        {
            // 获取新路径的终点（原地）
            double x_r, y_r, th_r;
            comp_traj->getEndpoint(x_r, y_r, th_r);
            // 坐标计算
            x_r += heading_lookahead_ * cos(th_r);
            y_r += heading_lookahead_ * sin(th_r);
            unsigned int cell_x, cell_y;
            // make sure that we'll be looking at a legal cell
            // 震荡抑制能够避免机器人在一个小范围内左右来回乱转。
            // 转换到地图坐标系，判断与目标点的距离
            if (costmap_.worldToMap(x_r, y_r, cell_x, cell_y))
            {
                double ahead_gdist = goal_map_(cell_x, cell_y).target_dist;
                // 取距离最小的，放进best_traj
                if (ahead_gdist < heading_dist)
                {
                    // if we haven't already tried rotating left since we've moved forward
                    if (vtheta_samp < 0 && !stuck_left)
                    {
                        swap = best_traj;
                        best_traj = comp_traj;
                        comp_traj = swap;
                        heading_dist = ahead_gdist;
                    }
                        // if we haven't already tried rotating right since we've moved forward
                    else if (vtheta_samp > 0 && !stuck_right)
                    {
                        swap = best_traj;
                        best_traj = comp_traj;
                        comp_traj = swap;
                        heading_dist = ahead_gdist;
                    }
                }
            }
        }
        // 角速度遍历
        vtheta_samp += dvtheta;
    }
    /* 轨迹生成已完成，接下来的工作可以概括为：
         如果轨迹cost非负，即找到有效轨迹，则将其返回；
         若找不到有效轨迹，进入脱困状态，后退、原地自转，
         若找不到下一步的有效路径则再后退、自转，
       直到后退的距离或转过的角度达到一定标准，才会退出脱困状态，重新规划前向轨迹。其中再加上震荡抑制。
     */
    // do we have a legal trajectory 检查最优轨迹的分数是否大于0
    if (best_traj->cost_ >= 0)
    {
        // avoid oscillations of in place rotation and in place strafing
        // 抑制震荡影响：当机器人在某方向移动时，对下一个周期的与其相反方向标记为无效
        // 直到机器人从标记震荡的位置处离开一定距离，返回最佳轨迹
        // 若轨迹的采样速度 > 0
        if (best_traj->xv_ <= 0)
        {
            // 若轨迹的角速度 < 0
            if (best_traj->thetav_ < 0)
            {
                if (rotating_right) stuck_right = true;
                rotating_right = true;
            }
            else if (best_traj->thetav_ > 0)
            {
                if (rotating_left) stuck_left = true;
                rotating_left = true;
            }
            else if (best_traj->yv_ > 0)
            {
                if (strafe_right) stuck_right_strafe = true;
                strafe_right = true;
            }
            else if (best_traj->yv_ < 0)
            {
                if (strafe_left) stuck_left_strafe = true;
                strafe_left = true;
            }
            // set the position we must move a certain distance away from
            prev_x_ = x;
            prev_y_ = y;
        }
        double dist = hypot(x - prev_x_, y - prev_y_);
        if (dist > oscillation_reset_dist_)
        {
            rotating_left = false;
            rotating_right = false;
            strafe_left = false;
            strafe_right = false;
            stuck_left = false;
            stuck_right = false;
            stuck_left_strafe = false;
            stuck_right_strafe = false;
        }
        dist = hypot(x - escape_x_, y - escape_y_);
        if (dist > escape_reset_dist_ ||
            fabs(angles::shortest_angular_distance(escape_theta_, theta)) > escape_reset_theta_) escaping_ = false;
        return *best_traj;
    }
    // only explore y velocities with holonomic robots 只研究全向机器人的速度
    if (holonomic_robot_)
    {
        // if we can't rotate in place or move forward... maybe we can move sideways and rotate
        vtheta_samp = min_vel_theta;
        vx_samp = 0.0;
        // loop through all y velocities
        for (double y_vel : y_vels_)
        {
            vtheta_samp = 0;
            vy_samp = y_vel;
            // sample completely horizontal trajectories
            generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
                               acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
            // if the new trajectory is better... let's take it
            if (comp_traj->cost_ >= 0 && (comp_traj->cost_ <= best_traj->cost_ || best_traj->cost_ < 0))
            {
                double x_r, y_r, th_r;
                comp_traj->getEndpoint(x_r, y_r, th_r);
                x_r += heading_lookahead_ * cos(th_r);
                y_r += heading_lookahead_ * sin(th_r);
                unsigned int cell_x, cell_y;
                // make sure that we'll be looking at a legal cell
                if (costmap_.worldToMap(x_r, y_r, cell_x, cell_y))
                {
                    double ahead_gdist = goal_map_(cell_x, cell_y).target_dist;
                    if (ahead_gdist < heading_dist)
                    {
                        // if we haven't already tried strafing left since we've moved forward
                        if (vy_samp > 0 && !stuck_left_strafe)
                        {
                            swap = best_traj;
                            best_traj = comp_traj;
                            comp_traj = swap;
                            heading_dist = ahead_gdist;
                        }
                            // if we haven't already tried rotating right since we've moved forward
                        else if (vy_samp < 0 && !stuck_right_strafe)
                        {
                            swap = best_traj;
                            best_traj = comp_traj;
                            comp_traj = swap;
                            heading_dist = ahead_gdist;
                        }
                    }
                }
            }
        }
    }
    // do we have a legal trajectory 是否找到了有效的局部路径
    if (best_traj->cost_ >= 0)
    {
        if (best_traj->xv_ <= 0)
        {
            if (best_traj->thetav_ < 0)
            {
                if (rotating_right) stuck_right = true;
                rotating_left = true;
            }
            else if (best_traj->thetav_ > 0)
            {
                if (rotating_left) stuck_left = true;
                rotating_right = true;
            }
            else if (best_traj->yv_ > 0)
            {
                if (strafe_right) stuck_right_strafe = true;
                strafe_left = true;
            }
            else if (best_traj->yv_ < 0)
            {
                if (strafe_left) stuck_left_strafe = true;
                strafe_right = true;
            }
            // set the position we must move a certain distance away from
            prev_x_ = x;
            prev_y_ = y;
        }
        double dist = hypot(x - prev_x_, y - prev_y_);
        if (dist > oscillation_reset_dist_)
        {
            rotating_left = false;
            rotating_right = false;
            strafe_left = false;
            strafe_right = false;
            stuck_left = false;
            stuck_right = false;
            stuck_left_strafe = false;
            stuck_right_strafe = false;
        }
        dist = hypot(x - escape_x_, y - escape_y_);
        if (dist > escape_reset_dist_ ||
            fabs(angles::shortest_angular_distance(escape_theta_, theta)) > escape_reset_theta_) escaping_ = false;
        return *best_traj;
    }
    // and finally, if we can't do anything else, we want to generate trajectories that move backwards slowly
    vtheta_samp = 0.0;
    vx_samp = backup_vel_; // 后退速度
    vy_samp = 0.0;
    generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
                       acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
    // if the new trajectory is better... let's take it
    /*
       if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
       swap = best_traj;
       best_traj = comp_traj;
       comp_traj = swap;
       }
       */
    // we'll allow moving backwards slowly even when the static map shows it as blocked
    swap = best_traj;
    best_traj = comp_traj;
    comp_traj = swap;
    double dist = hypot(x - prev_x_, y - prev_y_);
    if (dist > oscillation_reset_dist_)
    {
        rotating_left = false;
        rotating_right = false;
        strafe_left = false;
        strafe_right = false;
        stuck_left = false;
        stuck_right = false;
        stuck_left_strafe = false;
        stuck_right_strafe = false;
    }
    // only enter escape mode when the planner has given a valid goal point
    // 若后退速度生成的轨迹的终点有效（> -2.0），进入脱困状态，循环后退、自转，并且记录下的逃逸位置和姿态，
    // 只有当离开脱困位置一定距离或转过一定角度，才能退出脱困状态，再次规划前向速度。
    // 脱困判断和震荡判断一样，也已在上面多次执行，只要发布best_traj前就执行一次判断。
    if (!escaping_ && best_traj->cost_ > -2.0)
    {
        escape_x_ = x;
        escape_y_ = y;
        escape_theta_ = theta;
        escaping_ = true;
    }
    dist = hypot(x - escape_x_, y - escape_y_);
    if (dist > escape_reset_dist_ ||
        fabs(angles::shortest_angular_distance(escape_theta_, theta)) > escape_reset_theta_) escaping_ = false;
    // if the trajectory failed because the footprint hits something, we're still going to back up
    // 如果局部路径因为它的footprint撞到了障碍物而失败，也需要做一定的处理 若后退轨迹遇障，还是继续后退，因为后退一点后立刻就会进入原地自转模式。
    if (best_traj->cost_ == -1.0) best_traj->cost_ = 1.0;
    return *best_traj;
}

// 根据机器人当前的状态来计算找到较优的局部轨迹
// given the current state of the robot, find a good trajectory
Trajectory TrajectoryPlanner::findBestPath(const geometry_msgs::PoseStamped &global_pose,
                                           geometry_msgs::PoseStamped &global_vel,
                                           geometry_msgs::PoseStamped &drive_velocities)
{
    // 首先记录当前位姿(global系下)、速度，将机器人当前位置的足迹的标志位设置为真。
    // 并且，对path_map_和goal_map_的值重置后调用setTargetCells函数/setLocalGoal函数进行更新。
    // 这两个地图是局部规划器中专用的“地图”，即MapGrid类，和costmap的组织形式一样，都以cell为单位，
    // path_map_记录各cell与全局规划路径上的cell之间的距离，goal_map_记录各cell与目标cell之间的距离，
    // 再最终计算代价时，将这两个因素纳入考虑，以保证局部规划的结果既贴合全局规划路径、又不致偏离目标。

    // 将当前机器人位姿和速度转变成float形式的vector
    Eigen::Vector3f pos(global_pose.pose.position.x, global_pose.pose.position.y,
                        tf2::getYaw(global_pose.pose.orientation));
    Eigen::Vector3f vel(global_vel.pose.position.x, global_vel.pose.position.y,
                        tf2::getYaw(global_vel.pose.orientation));
    // 重置地图，清除所有障碍物信息以及地图内容 reset the map for new operations
    path_map_.resetPathDist();
    goal_map_.resetPathDist();
    // temporarily remove obstacles that are within the footprint of the robot 临时地删去机器人的footprint中的障碍物
    // 用当前位姿获取机器人footprint
    std::vector<base_local_planner::Position2DInt> footprint_list =
            footprint_helper_.getFootprintCells(pos, footprint_spec_, costmap_, true);
    // mark cells within the initial footprint of the robot 标记机器人的初始footprint内的单元格
    // 将初始footprint内的所有cell标记 “within_robot = true”,表示在机器人足迹内
    for (auto &i : footprint_list) path_map_(i.x, i.y).within_robot = true;
    // make sure that we update our path based on the global plan and compute costs
    // 确保根据全局路径来更新机器人的路径和计算代价
    // 更新哪些地图上的cell是在全局规划路径上的，target_dist设置为0
    // 并且通过它们和其他点的相对位置计算出来地图上所有点的target_dist
    path_map_.setTargetCells(costmap_, global_plan_);
    goal_map_.setLocalGoal(costmap_, global_plan_);
    ROS_DEBUG("Path/Goal distance computed");
    // rollout trajectories and find the minimum cost one
    // 滚动计算局部轨迹并且找到代价最小的一个，输入分别是目前机器人位置，速度以及加速度限制
    // 接下来，调用createTrajectories函数，传入当前位姿、速度、加速度限制，生成合理速度范围内的轨迹，并进行打分，结果返回至best。
    Trajectory best = createTrajectories(pos[0], pos[1], pos[2], vel[0], vel[1], vel[2], acc_lim_x_, acc_lim_y_, acc_lim_theta_);
    ROS_DEBUG("Trajectories created");
    /*
    //If we want to print a ppm file to draw goal dist
    char buf[4096];
    sprintf(buf, "base_local_planner.ppm");
    FILE *fp = fopen(buf, "w");
    if(fp){
      fprintf(fp, "P3\n");
      fprintf(fp, "%d %d\n", map_.size_x_, map_.size_y_);
      fprintf(fp, "255\n");
      for(int j = map_.size_y_ - 1; j >= 0; --j){
        for(unsigned int i = 0; i < map_.size_x_; ++i){
          int g_dist = 255 - int(map_(i, j).goal_dist);
          int p_dist = 255 - int(map_(i, j).path_dist);
          if(g_dist < 0)
            g_dist = 0;
          if(p_dist < 0)
            p_dist = 0;
          fprintf(fp, "%d 0 %d ", g_dist, 0);
        }
        fprintf(fp, "\n");
      }
      fclose(fp);
    }
    */
    // 对返回的结果进行判断，若其代价为负，表示说明所有的路径都不可用；若代价非负，表示找到有效路径，为drive_velocities填充速度后返回。
    if (best.cost_ < 0)
    {
        drive_velocities.pose.position.x = 0;
        drive_velocities.pose.position.y = 0;
        drive_velocities.pose.position.z = 0;
        drive_velocities.pose.orientation.w = 1;
        drive_velocities.pose.orientation.x = 0;
        drive_velocities.pose.orientation.y = 0;
        drive_velocities.pose.orientation.z = 0;
    }
    else
    {
        drive_velocities.pose.position.x = best.xv_;
        drive_velocities.pose.position.y = best.yv_;
        drive_velocities.pose.position.z = 0;
        tf2::Quaternion q;
        q.setRPY(0, 0, best.thetav_);
        tf2::convert(q, drive_velocities.pose.orientation);
    }
    return best;
}

// 当计算障碍物的代价时，机器人的footprint需要被考虑进去
//we need to take the footprint of the robot into account when we calculate cost to obstacles
double TrajectoryPlanner::footprintCost(double x_i, double y_i, double theta_i)
{
    // 检测机器人的footprint是否有效合法
    //check if the footprint is legal
    return world_model_.footprintCost(x_i, y_i, theta_i, footprint_spec_, inscribed_radius_, circumscribed_radius_);
}


void TrajectoryPlanner::getLocalGoal(double &x, double &y)
{
    x = path_map_.goal_x_;
    y = path_map_.goal_y_;
}

};
